#ifndef __UDSF_UART_C_
#define __UDSF_UART_C_

#include "udf_global.h"
/*
*   notice:
*   UDSF 同UDF编程：不可调用cstd-lib,只可调用"sdk_ifs_udk_cfg.h"、"sys_api.h"中列出的API函数。
*   UDSF文件内函数均需使用static修饰符，且UDF中以#include "UDSF.c"形式引用
*/

static void udsf_uart_send_byte(uint8_t data) __attribute__((unused));
static void udsf_uart_send_string(char* pStr) __attribute__((unused));
static void udsf_uart_init(uint32_t baudrate,uint8_t boot_flag) __attribute__((unused));
static void udsf_uart_send_data(uint8_t* pData,int data_len) __attribute__((unused));
static void udsf_uart_send_data_base16(uint8_t* pData,int data_len) __attribute__((unused));
static void udsf_uart_send_data_trans(uint8_t*pData,int data_len) __attribute__ ((unused));
static int  udsf_uart_recv_frame_data(uint8_t* pData,int max_len) __attribute__ ((unused));
static void udsf_uart_send_u32_base16(int endian,uint32_t* Data,int data_len,char* start,char*end) __attribute__ ((unused));
static void udsf_uart_send_u16_base16(int endian,uint16_t* Data,int data_len,char* start,char*end) __attribute__ ((unused));
static void udsf_uart_send_u8_base16(uint8_t* Data,int data_len,char* start,char*end) __attribute__ ((unused));
static void udsf_uart_recv_init(void) __attribute__ ((unused));
static void udsf_uart_recv_enable(void)  __attribute__((unused));
static void udsf_uart_recv_disable(void) __attribute__((unused));
static void udsf_clear_track_data(TrackInfo *track_clear) __attribute__((unused));
static void udsf_track_FastIn(MaxPowPoint *clusterP, TrackInfo *track, PARAM_RSV * params, uint8_t BodyInSign) __attribute__((unused));

static void udsf_track_FastIn(MaxPowPoint *clusterP, TrackInfo *track, PARAM_RSV * params, uint8_t BodyInSign)
{
	int32_t alpha = 5; // 以整数为单位参与计算实际为0.8 最后需要除以10
	//int32_t beta = 1;
	
	int32_t Frame_T = 100;  //ms为单位参与计算
	int32_t r_predict;
	//int32_t delta_r;
	int32_t devData;
	int16_t devData_r, devData_v, devData_a;
	//uint8_t trigerDistance[10] = {50, 60, 70, 80, 90, 100, 110, 120, 130, 140};// 设置的触发距离(不包括雷达到马桶前沿的50cm)
	//uint8_t trigerDistance[10] = {129, 141, 153, 165, 176, 188, 200, 212, 224, 235};// (设置的触发距离+50+10)/0.85, 进行30°安装角度折算及实测触发偏晚的补偿
//#define DEBUG_TRACKINFO
#ifdef DEBUG_TRACKINFO
	printf("Before---track_sign = %d, cluter = %d\r\n", track->track_sign, clusterP->maxPowSign);
#endif
	
	if(track->track_sign == 1 && clusterP->maxPowSign == 0)
	{
		//track->track_v = 20;
		track->track_r = track->track_r + track->track_v * Frame_T / 1000;
		track->track_rY = track->track_r * track->track_a/100;
		
		if(track->track_cnt > 0)
			track->track_cnt--;

		track->lost_cnt++;
		
		if(track->lost_cnt > TRACK_LOST_COUNT || track->lost_cnt > (track->track_cnt+2))
		{
			udsf_clear_track_data(track);
		}
	}
	else if(track->track_sign == 1 && clusterP->maxPowSign == 1)
	{
		r_predict = track->track_r + track->track_v*Frame_T/1000;
		//delta_r = clusterP->maxPowR - r_predict;
		devData_r = (int16_t)clusterP->maxPowR - (int16_t)r_predict;
		devData_v = ((int16_t)clusterP->maxPowV - (int16_t)track->track_v);
		devData_a = ((int16_t)clusterP->maxPowA - (int16_t)track->track_a);// 角度影响比较大，故乘了1.3的系数
		devData = devData_r*devData_r + devData_v*devData_v + devData_a*devData_a;
		
#ifdef DEBUG_TRACKINFO
		printf("r_p = %d, devData = %d\r\n", r_predict, devData);
#endif

		if(devData <= FRAME_COV_LIMIT)
		{
			track->track_r = r_predict + alpha * devData_r / 10;
			//track->track_v = track->track_v + beta * devData_r * 100 / Frame_T;
			track->track_v = (track->track_v + clusterP->maxPowV) / 2;
			track->track_a = (track->track_a + clusterP->maxPowA) / 2;
			track->track_sign = 1;
			track->track_cnt++;
			track->lost_cnt = 0;
		}
		else
		{
			track->track_r = r_predict;
			
			if(track->track_cnt > 0)
				track->track_cnt--;
			
			track->lost_cnt++;
			
			if(track->lost_cnt > TRACK_LOST_COUNT || track->lost_cnt > (track->track_cnt+2))
			{
				udsf_clear_track_data(track);
			}
		}
		track->track_rY = track->track_r * track->track_a/100;
	}
	else if(track->track_sign == 0 && clusterP->maxPowSign == 0)
	{
		udsf_clear_track_data(track);
	}
	else if(track->track_sign == 0 && clusterP->maxPowSign == 1)
	{
		track->track_startCnt++;
		devData_r = (int16_t)clusterP->maxPowR - (int16_t)track->track_r;
		devData_v = ((int16_t)clusterP->maxPowV - (int16_t)track->track_v);
		devData_a = ((int16_t)clusterP->maxPowA - (int16_t)track->track_a);
		devData = devData_r*devData_r + devData_v*devData_v + devData_a*devData_a;
		track->track_r = clusterP->maxPowR;
		track->track_v = clusterP->maxPowV;
		track->track_a = clusterP->maxPowA;
		track->track_rY = track->track_r * track->track_a/100;
		if(track->track_startCnt > 2 && devData <= FRAME_COV_LIMIT)
		{
			if(track->track_v > VELTHRES*7 && track->track_v < VELTHRES*16)
			{
				track->track_v = 200;
			}
			else if(track->track_v > -VELTHRES*16 && track->track_v < -VELTHRES*8)
			{
				track->track_v = -240;
			}
			
			track->track_startCnt = 0;
			track->track_sign = 1;
		}
	}
	
#ifdef DEBUG_TRACKINFO
	printf("t_r = %d, t_v = %d, t_a = %d\r\n", track->track_r, track->track_v, track->track_a);
	printf("startCnt = %d, track_cnt = %d, track_lost = %d\r\n", track->track_startCnt, track->track_cnt, track->lost_cnt);
	printf("After---track_sign = %d, cluster = %d\r\n", track->track_sign, clusterP->maxPowSign);
#endif
}
static void udsf_clear_track_data(TrackInfo *track_clear)
{
	track_clear->track_sign = 0;
	track_clear->track_cnt = 0;
	track_clear->lost_cnt = 0;
	track_clear->track_r = 0;
	track_clear->track_v = 0;
	track_clear->track_a = 0;
	track_clear->track_rY = 0;
	track_clear->track_startCnt = 0;
}
static void udsf_uart_init(uint32_t baudrate,uint8_t boot_flag){
	
	uint32_t raw_baud;
	
	if( boot_flag == 0 ){
		 raw_baud = 640000000/baudrate;
	}else{
		raw_baud = 320000000/baudrate;
	}

	uint32_t baud_div = raw_baud>>4;
	uint32_t baud_mod = ((raw_baud>>1)&0x7) + (raw_baud&0x1);
	USCI1->MOD = (USCI_BAUD_SDIV_Msk & (baud_mod << USCI_BAUD_SDIV_Pos));
	USCI1->MOD &= ~USCI_BAUD_S16_Msk;
	USCI1->BAUD = USCI_BAUD_DIV_Msk &((baud_div>0)?(baud_div-1):(baud_div));
	;// code@rom init uart already,just change baudrate
}

static void udsf_uart_send_byte(uint8_t data){
	USCI1->TRX = data;
	while((Read_M32(0x40004014)&USCI_ISR_TXE_Msk)==0); //USCI1 ISR		
}

static void udsf_uart_send_string(char* pStr){
	
		
	USCI1->ISR = USCI_ISR_TXE_Msk | USCI_ISR_TE_Msk;
	int i;
	for( i=0;;i++ ){
		if( pStr[i] == '\0' ){
			break;
		}
		USCI1->TRX = pStr[i];
		while((Read_M32(0x40004014)&USCI_ISR_TXE_Msk)==0);
	}
	if( i>0){
		while((USCI1->ISR&USCI_ISR_TE_Msk)==0);
		USCI1->ISR = USCI_ISR_TXE_Msk | USCI_ISR_TE_Msk;
	}
}

static void udsf_uart_send_data(uint8_t* pData,int data_len){
	
	if( data_len < 1 ){
		return;
	}
	USCI1->ISR = USCI_ISR_TXE_Msk | USCI_ISR_TE_Msk;
	for( int i=0;i<data_len;i++ ){
		USCI1->TRX = pData[i];
		while((Read_M32(0x40004014)&USCI_ISR_TXE_Msk)==0);
	}
	while((USCI1->ISR&USCI_ISR_TE_Msk)==0);
	USCI1->ISR = USCI_ISR_TXE_Msk | USCI_ISR_TE_Msk;
}

/*send data and 0xff->0xfd 0x02 0xfe->0xfd 0x01 0xfd->0xfd 0x00 */
static void udsf_uart_send_data_trans(uint8_t*pData,int data_len){
	uint8_t data;

	if( data_len < 1 ){
		return;
	}
	USCI1->ISR = USCI_ISR_TXE_Msk | USCI_ISR_TE_Msk;
	
	for( int i=0;i<data_len;i++ ){
		if( pData[i] > 0xfc ){
			USCI1->TRX = 0xfd;
			data = pData[i] - 0xfd;
			while((Read_M32(0x40004014)&USCI_ISR_TXE_Msk)==0);
			USCI1->TRX = data;
		}
		else{
			USCI1->TRX = pData[i];
		}
		while((Read_M32(0x40004014)&USCI_ISR_TXE_Msk)==0);
	}
	
	while((USCI1->ISR&USCI_ISR_TE_Msk)==0);
	USCI1->ISR = USCI_ISR_TXE_Msk | USCI_ISR_TE_Msk;
}

/*endian: 0 -> little-endian 1-> big-enndian*/
static void udsf_uart_send_u32_base16(int endian,uint32_t* Data,int data_len,char* start,char*end){

	uint8_t data[4];
	if( start ){
		udsf_uart_send_string(start);
	}
	for ( int i = 0;i<data_len;i++){
		if( endian == 0 ){
			*((uint32_t*)(&data)) = Data[i];
		}else{
			data[0] = Data[i]>>24;
			data[1] = Data[i]>>16;
			data[2] = Data[i]>>8;
			data[3] = Data[i];
		}
		udsf_uart_send_data_base16(data,sizeof(data));
	}
	if( end ){
		udsf_uart_send_string(end);
	}
}

/*endian: 0 -> little-endian 1-> big-enndian*/
static void udsf_uart_send_u16_base16(int endian,uint16_t* Data,int data_len,char* start,char*end){

	uint8_t data[2];
	if( start ){
		udsf_uart_send_string(start);
	}
	for ( int i = 0;i<data_len;i++){
		if( endian == 0 ){
			*((uint16_t*)(&data)) = Data[i];
		}else{
			data[0] = Data[i]>>8;
			data[1] = Data[i];
		}
		udsf_uart_send_data_base16(data,sizeof(data));
	}
	if( end ){
		udsf_uart_send_string(end);
	}
}


/*endian: 0 -> little-endian 1-> big-enndian*/
static void udsf_uart_send_u8_base16(uint8_t* Data,int data_len,char* start,char*end){

	if( start ){
		udsf_uart_send_string(start);
	}
	
	udsf_uart_send_data_base16(Data,data_len);
	
	if( end ){
		udsf_uart_send_string(end);
	}
}


static void udsf_uart_send_data_base16(uint8_t* pData,int data_len){

	uint8_t tmp_dataA,tmp_dataB,tmp_out;

	if( data_len < 1 ){
		return;
	}
	USCI1->ISR = USCI_ISR_TXE_Msk | USCI_ISR_TE_Msk;
	
	for( int i=0;i<data_len;i++ ){
		tmp_dataA = (pData[i]&0xf0)>>4;
		if( tmp_dataA > 9 ){
			tmp_out = 'A'+(tmp_dataA-10);
		}else{
			tmp_out = '0'+tmp_dataA;
		}
		USCI1->TRX = tmp_out;  // send high-4-bit
		
		tmp_dataB = (pData[i]&0x0f);
		if( tmp_dataB > 9 ){
			tmp_out = 'A'+(tmp_dataB-10);
		}else{
			tmp_out = '0'+tmp_dataB;
		}
		while((Read_M32(0x40004014)&USCI_ISR_TXE_Msk)==0);
		USCI1->TRX = tmp_out;  // send low-4-bit
		while((Read_M32(0x40004014)&USCI_ISR_TXE_Msk)==0);
		USCI1->TRX = ' ';      // send space
		while((Read_M32(0x40004014)&USCI_ISR_TXE_Msk)==0);
	}
	
	while((USCI1->ISR&USCI_ISR_TE_Msk)==0);
	USCI1->ISR = USCI_ISR_TXE_Msk | USCI_ISR_TE_Msk;
}

struct uart_rom_def
{
	uint8_t rsv1;  
	uint32_t len;     // 帧长度
	uint8_t flag;     // 接收有效帧标识
	uint8_t rsv2;
	uint8_t data[50]; // 缓存区
	uint8_t rsv3;
	uint32_t rsv4;
	uint32_t rsv5;
};


static void udsf_uart_recv_init(void){
	/* disable uart irq */
	NVIC->ICER[0] = (1 << ((uint32_t)(USCI1_IRQn) & 0x1F));
	USCI1->IMR=0xFFFFFFFF;	

	uint8_t * p_rom_global_workmode = p_work_mode;	
	/* set workmode->fake config mode */
	*p_rom_global_workmode = 0;

	/* enable uart irq */
	USCI1->ISR = 0x1F;
	USCI1->IMR &= ~USCI_IMR_RXNE_Msk;
	NVIC->ISER[0] = (1 << ((uint32_t)(USCI1_IRQn) & 0x1F));
}

static int udsf_uart_recv_frame_data(uint8_t* pData,int max_len){
	
	int len=-1;
	
	#define p_rom_global_uart_struct ((struct uart_rom_def*)UART_RECV_STRUCT_BASE)
		
	if( p_rom_global_uart_struct->flag ){
	
		len = p_rom_global_uart_struct->len;
		len = len>max_len?max_len:len;
		for( int i=0;i<len;i++){
			pData[i] = p_rom_global_uart_struct->data[i];
		}
		p_rom_global_uart_struct->len = 0;
		p_rom_global_uart_struct->flag = 0;
	}
	#undef p_rom_global_uart_struct
	return len;
}
//static int udsf_uart_recv_frame_data_irq(uint8_t* pData,int max_len){
//	
//	int len=-1;
//	#define p_rom_global_uart_struct ((struct uart_rom_def*)UART_RECV_STRUCT_BASE)

//	if(p_rom_global_uart_struct->len>0)
//	{
//		len = p_rom_global_uart_struct->len;
//		len = len>max_len?max_len:len;	
//		for( int i=0;i<len;i++){
//			pData[i] = p_rom_global_uart_struct->data[i];
//		}

//		if(pData[0]==0XAA&&pData[1]==0X11)
//		{	
//			pData[0]  = 0XCC;
//			pData[1]  = 0X11;
//			pData[2]  = 0X08;
//			pData[3]  = 0X01;
//			pData[4]  = 0X00;//软件次版本号
//			pData[5]  = 0X01;//软件主版本号
//			pData[6]  = 0X00;//硬件次版本号
//			pData[7]  = 0X01;//硬件主版本号
//			pData[8]  = 0X15;//模块识别码0X20 加特兰 0X21电目 0X22本控
//			pData[9]  = 0X01;//读取当前工作状态 0 ：雷达完成面积检测。 1 ：雷达正在进行面积检测
//			pData[10] = 0X00;//读取毫米波模块是否开启0，0：未开启1-开启(需要修改的话，通过0X24命令修改)
//			pData[11] = 0X00;//当前的灵敏度：0x20 宠物高灵敏度 0x10 人普通灵敏度
//			pData[12] = 0X00;
//			for(uint8_t i = 0;i<12;i++)
//			{
//				pData[12]+= pData[i];
//			}
//			pData[13] = 0XDD;
//			udsf_uart_send_data(pData,14);
//			pData[12] = 0;
//			for (int k = 0; k < 1500; k++)
//			{			 
//				for (int j = 0; j < 950; j++)
//				{
//					for(int i = 0; i < 6; i++);
//				}
//			}
//			p_rom_global_uart_struct->len = 0;
//		}
//		if(pData[0]==0XAA&&pData[1]==0X62&&pData[13]==0XBB)
//		{
//      paramRSV->motion_hold_sec	= 1;		
//			p_rom_global_uart_struct->len = 0;
//		}
//		p_rom_global_uart_struct->len = 0;
//		p_rom_global_uart_struct->flag = 0;  
//		p_rom_global_uart_struct->rsv3 = 0;
//	}
//	#undef p_rom_global_uart_struct
//	return len;
//}
static void udsf_uart_recv_disable(void){
	NVIC->ICER[0] = (1 << ((uint32_t)(USCI1_IRQn) & 0x1F));
	USCI1->IMR=0xFFFFFFFF;	
}

static void udsf_uart_recv_enable(void){
	USCI1->ISR = 0x1F;
	USCI1->IMR &= ~USCI_IMR_RXNE_Msk;
	NVIC->ISER[0] = (1 << ((uint32_t)(USCI1_IRQn) & 0x1F));
}

#endif

